#include "stdafx.h"

#include <Common/Base/keycode.cxx>

#ifdef HK_FEATURE_PRODUCT_ANIMATION
	#undef HK_FEATURE_PRODUCT_ANIMATION
#endif

#ifndef HK_EXCLUDE_LIBRARY_hkgpConvexDecomposition
	#define HK_EXCLUDE_LIBRARY_hkgpConvexDecomposition
#endif

#include <Common/Base/Config/hkProductFeatures.cxx> 

#include <Physics/Collide/hkpCollide.h>	

struct hkTestEntry* hkUnitTestDatabase = HK_NULL; 
hkBool HK_CALL hkTestReport(hkBool32 cond, const char* desc, const char* file, int line) {return false;}
#include "HavokUtilities.h"

HavokUtilities* HavokUtilities::instance = NULL;

HavokUtilities* HavokUtilities::getInstance() {
	if (!instance) {
		instance = new HavokUtilities(true);
	}

	return instance;
}

void HavokUtilities::createPlane(float x, float y, float z, float width, float height, float thickness) {
	addFixedSurface( hkVector4(x, y, z), hkVector4( width, thickness, height ));
}

HavokUtilities::HavokUtilities(bool visualDebuggerActive) {
	// Sets Havok's gravity to zero to implement our own planetary gravity
	gravityForce = 0.0f;
	
	m_bVisualDebuggerActive = visualDebuggerActive;

	if (visualDebuggerActive==true) {
		#define HAVOK_VISUAL_DEBUGGER_ENABLED 1
	}

	initHavok();
}

HavokUtilities::~HavokUtilities(void) {
	deinitHavok();
}

void HavokUtilities::initHavok() {
	m_pMemoryRouter = hkMemoryInitUtil::initDefault(hkMallocAllocator::m_defaultMallocAllocator, hkMemorySystem::FrameInfo(500000));
	hkBaseSystem::init( m_pMemoryRouter, errorReport );

	hkGetHardwareInfo(m_hardwareInfo);
	m_iTotalNumThreadsUsed = m_hardwareInfo.m_numThreads;

	m_threadPoolCinfo.m_numThreads = m_iTotalNumThreadsUsed - 1;

	m_threadPoolCinfo.m_timerBufferPerThreadAllocation = 200000;
	m_pThreadPool = new hkCpuJobThreadPool( m_threadPoolCinfo );

	m_jobQueuInfo.m_jobQueueHwSetup.m_numCpuThreads = m_iTotalNumThreadsUsed;
	m_pJobQueue = new hkJobQueue(m_jobQueuInfo);

	hkMonitorStream::getInstance().resize(200000);

	m_worldInfo.m_simulationType = hkpWorldCinfo::SIMULATION_TYPE_MULTITHREADED;
	m_worldInfo.m_broadPhaseBorderBehaviour = hkpWorldCinfo::BROADPHASE_BORDER_REMOVE_ENTITY;
	m_worldInfo.m_gravity.set(0,gravityForce,0);
	m_worldInfo.m_collisionTolerance = 0.1f;
	m_worldInfo.setBroadPhaseWorldSize(5000.0f);
	m_worldInfo.m_contactPointGeneration = hkpWorldCinfo::CONTACT_POINT_ACCEPT_ALWAYS;

	
		
		
	
		


	m_pPhysicsWorld = new hkpWorld(m_worldInfo);
	m_pPhysicsWorld->m_wantDeactivation = false;

	m_pPhysicsWorld->markForWrite();
	hkpAgentRegisterUtil::registerAllAgents( m_pPhysicsWorld->getCollisionDispatcher() );
	m_pPhysicsWorld->registerWithJobQueue( m_pJobQueue );
	m_pPhysicsWorld->unmarkForWrite();
	
	#ifdef HAVOK_VISUAL_DEBUGGER_ENABLED
		registerVisualDebugger();
	#endif	
}

void HavokUtilities::deinitHavok() {
	// Clean up physics world
	m_pPhysicsWorld->markForWrite();
	m_pPhysicsWorld->removeReference();

	// Clean up visual debugger (if used)
	#ifdef HAVOK_VISUAL_DEBUGGER_ENABLED
		m_pVisualDebugger->removeReference();
		m_pPhysicsContext->removeReference();
	#endif	

	// Delete job queue, thread poll, deallocate memory
	delete m_pJobQueue;
	m_pThreadPool->removeReference();

	// Quit base system, this also deallocates the buffer which is used for Havok
	hkBaseSystem::quit();
}

void HavokUtilities::registerVisualDebugger() {
	m_pPhysicsContext = new hkpPhysicsContext();
	hkpPhysicsContext::registerAllPhysicsProcesses();
		
	m_pPhysicsWorld->markForWrite();
	m_pPhysicsContext->addWorld(m_pPhysicsWorld);
	m_arrayPhysicsContext.pushBack(m_pPhysicsContext);
	m_pPhysicsWorld->unmarkForWrite();

	m_pVisualDebugger = new hkVisualDebugger(m_arrayPhysicsContext);
	m_pVisualDebugger->serve();
}

void HavokUtilities::stepSimulation(float deltaTime) {
	m_pPhysicsWorld->stepMultithreaded(m_pJobQueue, m_pThreadPool, deltaTime);
	stepVisualDebugger(deltaTime);
	hkMonitorStream::getInstance().reset();
	m_pThreadPool->clearTimerData();		
}

void HavokUtilities::stepVisualDebugger(float deltaTime) {
	#ifdef HAVOK_VISUAL_DEBUGGER_ENABLED
		m_pPhysicsContext->syncTimers( m_pThreadPool );
		m_pVisualDebugger->step();
	#endif	
}

hkpWorld* HavokUtilities::getWorld() {
	return m_pPhysicsWorld;
}

hkVisualDebugger* HavokUtilities::getVisualDebugger() {
	return m_pVisualDebugger;
}

void HavokUtilities::addFixedSurface(const hkVector4& position, const hkVector4& dimensions) {
	hkReal m_fhkConvexShapeRadius=0.05;
	hkpShape* fixedSurfaceShape = new hkpBoxShape(dimensions,m_fhkConvexShapeRadius);

	hkpRigidBodyCinfo m_rigidBodyInfo;
	
	m_rigidBodyInfo.m_mass = 0.0;
	m_rigidBodyInfo.m_shape = fixedSurfaceShape;
	m_rigidBodyInfo.m_motionType = hkpMotion::MOTION_FIXED;
	m_rigidBodyInfo.m_position = position;
	hkVector4 m_fAxis = hkVector4(0.0,0.0,1.0);
	hkReal m_fAngle=0.0f;
	m_rigidBodyInfo.m_rotation = hkQuaternion(m_fAxis,m_fAngle);

	hkpRigidBody* m_pRigidBody = new hkpRigidBody(m_rigidBodyInfo);

	m_pPhysicsWorld->lock();
	m_pPhysicsWorld->addEntity(m_pRigidBody);

	m_pRigidBody->removeReference();
	fixedSurfaceShape->removeReference();

	m_pPhysicsWorld->unlock();
}

void HavokUtilities::addMovingBoxes(const hkVector4& position, const hkVector4& dimensions) {
	hkReal m_fhkConvexShapeRadius=0.05;
	hkpShape* movingBodyShape = new hkpBoxShape(dimensions,m_fhkConvexShapeRadius);

	hkpMassProperties m_massProperties;
	hkReal m_massOfBox = 5.0;
	hkpInertiaTensorComputer::computeShapeVolumeMassProperties(movingBodyShape, m_massOfBox, m_massProperties);

	hkpRigidBodyCinfo m_rigidBodyInfo;
	
	m_rigidBodyInfo.m_position = position;
	m_rigidBodyInfo.m_mass = m_massProperties.m_mass;
	m_rigidBodyInfo.m_centerOfMass = m_massProperties.m_centerOfMass;
	m_rigidBodyInfo.m_inertiaTensor = m_massProperties.m_inertiaTensor;
	m_rigidBodyInfo.m_shape = movingBodyShape;
	m_rigidBodyInfo.m_motionType = hkpMotion::MOTION_BOX_INERTIA;

	hkpRigidBody* m_pRigidBody = new hkpRigidBody(m_rigidBodyInfo);

	m_pPhysicsWorld->lock();
	m_pPhysicsWorld->addEntity(m_pRigidBody);

	m_pRigidBody->removeReference();
	movingBodyShape->removeReference();

	m_pPhysicsWorld->unlock();
}

bool HavokUtilities::update(float deltaTime) {
	if(deltaTime > 0) {
		stepSimulation(deltaTime);
	}

	return true;
}